// ESC library

#ifndef H_ESC
#define H_ESC

#include "Arduino.h"

#define MAXCHANOUT 6
#define FRONTL 0
#define FRONTR 1
#define REARL  2
#define REARR  3

#define LEFT   4   // channel D5 or A0
#define RIGHT  5   // channel D6 or A1

#define REAR  4   // channel D5 or A0
#define FRONT   5   // channel D6 or A1

#define CAMPITCH 4 // channel D5 or A0
#define CAMROLL 5  // channel D6 or A1

#if defined(STANDARD)  || PIPO==2

#define PWM_FRONTL 3
#define PWM_FRONTR 10
#define PWM_REARL  11
#define PWM_REARR  9

#else

#error ERROR esc.h: Board not compatible // TODO

#endif
/****************** ESC *************************/
class Esc
{
private:
  byte state; //0=inactive, 1=active
public:
  float channelOut[MAXCHANOUT];
  int ichannelOut[MAXCHANOUT];

  void setup()
  {
    pinMode(PWM_FRONTL, OUTPUT); // OSC1A
    pinMode(PWM_FRONTR, OUTPUT); // OSC1B

    pinMode(PWM_REARL, OUTPUT); // OSC2A
    pinMode(PWM_REARR, OUTPUT); // OSC2B

    state=0;
    update();
  }

  void setState(byte _state)
  {
    state=_state;
  }

  void update()
  {

    unsigned int period;

    for(byte i=0;i<MAXCHANOUT;i++)
    {
#ifdef CAMSTAB
        if (state==0 && i<4) // TODO check if 4 is always ok
#else
        if (state==0)
#endif
          ichannelOut[i]=1000;
        else 
          ichannelOut[i]=limit(1150,channelOut[i]*500+CENTER,2000);
    }

    analogWrite(PWM_FRONTL,ichannelOut[FRONTL]>>3); 
    analogWrite(PWM_REARR,ichannelOut[REARR]>>3); 
    analogWrite(PWM_FRONTR,ichannelOut[FRONTR]>>3); 
    analogWrite(PWM_REARL,ichannelOut[REARL]>>3); 
    // other 2 channels are present in servo.h

  }
} 
esc;

#endif





